#include "MeOrion.h"
#include <SoftwareSerial.h>
#include "crc16.h"

/***********************************/
/*  1区                      4区   */
/*        _________________        */
/*        |               |        */
/*        |               |        */
/*        |               |        */
/*        |_______________|        */
/*                                 */
/*  2区                      3区   */
/***********************************/

/***********************************/
/*       比赛场地的真实大小        */
/***********************************/
#define PLAYGROUD_REAL_WIDTH 8.0f
#define PLAYGROUD_REAL_HEIGHT 8.0f

/***********************************/
/*   比赛场地在屏幕上的像素大小    */
/***********************************/
#define PLAYGROUD_WIDTH 240    
#define PLAYGROUD_HEIGHT 240   

/***********************************/
/*          TFT LCD屏接口          */
/* 分辨率320x240，横轴为X，纵轴为Y */
/***********************************/
MeSerial lcdSerial(PORT_6); 

/***********************************/
/*       机器人位姿状态变量        */
/***********************************/
/***********************************/
/*         Mecanum Wheel           */
/*    Power Transmission System    */
/***********************************/
/*          2        1             */
/*              |y                 */
/*             b|___x              */
/*           z    a                */
/*          3        4             */
/***********************************/
int px = 0;
int py = 0;
int pz = 0;
int vx = 0;
int vy = 0;
int vz = 0;

/***********************************/
/*       机器人状态接收串口        */
/***********************************/
MeSerial mySerial(PORT_3);

/***********************************/
/*           蓝牙串口              */
/***********************************/
MeBluetooth bluetooth(PORT_4);

/***********************************/
/*          红外遥控接口           */
/***********************************/
MeInfraredReceiver IR(PORT_5);

/***********************************/
/*              清屏               */
/***********************************/
void clearScreen(void)
{
   lcdSerial.println("CLS(0);"); // clear the screen with c color 
}

/***********************************/
/*         切换到横屏模式          */
/***********************************/
void rotateScreen(void)
{
  lcdSerial.println("DR0;");    // the screen displays in upright way
}

/***********************************/
/*            绘制网格             */
/***********************************/
void drawGrid(void)
{
  char fmt[50];
  for(int i = 0; i < 9; i++)
  {
    sprintf(fmt, "PL(%d,%d,%d,%d,%d);", 80, i*30, 320, i*30, 4);
    lcdSerial.print(fmt);
    sprintf(fmt, "PL(%d,%d,%d,%d,%d);", 80+i*30, 0, 80+i*30, 240, 4);
    lcdSerial.println(fmt);
  }
}

/***********************************/
/*            绘制边框             */
/***********************************/
void drawBorder(void)
{
  lcdSerial.println("BOX(80,0,320,240,4);");
}

/***********************************/
/*            绘制01区             */
/***********************************/
void drawArea1(void)
{
  lcdSerial.println("BOX(80,0,140,60,2);");
}

/***********************************/
/*            绘制02区             */
/***********************************/
void drawArea2(void)
{
  lcdSerial.println("BOX(80,180,140,239,2);");
}

/***********************************/
/*            绘制03区             */
/***********************************/
void drawArea3(void)
{
  lcdSerial.println("BOX(260,180,319,239,2);");
}

/***********************************/
/*            绘制04区             */
/***********************************/
void drawArea4(void)
{
  lcdSerial.println("BOX(260,0,319,60,4);");
}

/***********************************/
/*            绘制05区             */
/***********************************/
void drawArea5(void)
{
  lcdSerial.println("BOX(140,60,260,180,4);");
}

/***********************************/
/*           绘制比赛场地          */
/***********************************/
void drawPlaygroud(void)
{
  //Step 1: 清屏
  clearScreen();
  //Step 2: 旋转屏幕
  rotateScreen();
  //Step 3: 绘制网格
  //drawGrid();
  //Step 4: 绘制边框
  //drawBorder();
  //Step 5: 绘制01区
  drawArea1();
  //Step 6: 绘制02区
  drawArea2();
  //Step 7: 绘制03区
  drawArea3();
  //Step 8: 绘制04区
  drawArea4();
  //Step 9: 绘制05区
  drawArea5();
}

/***********************************/
/*       绘制机器人位姿        */
/***********************************/
void drawMe(void)
{
  char fmt[40];
  sprintf(fmt, "DS16(0,0,'px=%6d',4);", px);
  lcdSerial.print(fmt);
  sprintf(fmt, "DS16(0,32,'py=%6d',4);", py);
  lcdSerial.print(fmt);
  sprintf(fmt, "DS16(0,64,'pz=%6d',4);", pz);
  lcdSerial.print(fmt);
  sprintf(fmt, "DS16(0,96,'vx=%6d',4);", vx);
  lcdSerial.print(fmt);
  sprintf(fmt, "DS16(0,128,'vy=%6d',4);", vy);
  lcdSerial.print(fmt);
  sprintf(fmt, "DS16(0,160,'vz=%6d',4);", vz);
  lcdSerial.print(fmt);
  sprintf(fmt, "CIR(%d,%d,%d,61444);", (int)(200+px/80), (int)(120-py/80), (int)(2+sqrt(vx*vx+vy*vy+vz*vz)/1000));
  lcdSerial.println(fmt);
}

void setup() 
{
  lcdSerial.begin(9600);   //配置TFT LCD串口波特率
  bluetooth.begin(115200); //配置蓝牙串口波特率
  drawPlaygroud();         //绘制比赛场地
  delay(10);
  drawMe();                //绘制机器人位姿
  delay(10);
   mySerial.begin(115200);  //配置梯形板串口波特率
  Serial.begin(115200);    //配置PC串口波特率
}

#define MSG_LEN 16
#define MSG_HEADER 0xabcd
#define MSG_CRC16 0xdcba
#define BUF_LEN 20
uint8_t buf[BUF_LEN];
uint16_t r = 0;
uint16_t w = 0;
void process(uint8_t data)
{
  buf[w++] = data;
  if(w == BUF_LEN)
  {
    for(int i = r; i < w; i++)
    {
      buf[i-r] = buf[i];
    }
    w-= r;
    r = 0;
  }
  if(w-r >= MSG_LEN)
  {
    if(((uint16_t*)(buf+r))[0] == MSG_HEADER)
    {
      //Serial.println("header check sucess!");
      if(CRC16Check(buf+r,MSG_LEN,MSG_CRC16))
      {
        //Serial.println("packet check failed!");
        
        px = ((int16_t*)(buf+r))[1];
        py = ((int16_t*)(buf+r))[2];
        pz = ((int16_t*)(buf+r))[3];
        vx = ((int16_t*)(buf+r))[4];
        vy = ((int16_t*)(buf+r))[5];
        vz = ((int16_t*)(buf+r))[6];

        drawMe();
        r+=MSG_LEN;
        return;
      }
      else
      {
        //Serial.println("packet check failed!");
        r++;
        return;
      }
    }
    else
    {
      //Serial.println("header check failed!");
      r++;
      return;
    }
  }
}

void loop() 
{
  if(mySerial.available())
  {
    uint8_t data = mySerial.read();
    Serial.write(data);
    process(data);
  }
   if(IR.available())
  {
    uint8_t IRCode = IR.read();
    switch(IRCode)
    {
       case IR_BUTTON_A: Serial.println("Press A."); break;
       case IR_BUTTON_B: Serial.println("Press B."); break;
       case IR_BUTTON_C: Serial.println("Press C."); break;
       case IR_BUTTON_D: Serial.println("Press D."); break;
       case IR_BUTTON_E: Serial.println("Press E."); break;
       case IR_BUTTON_F: Serial.println("Press F."); break;
       case IR_BUTTON_SETTING: Serial.println("Press Setting."); break;
       case IR_BUTTON_UP: Serial.println("Press Up."); break;
       case IR_BUTTON_DOWN: Serial.println("Press Down."); break;
       case IR_BUTTON_LEFT: Serial.println("Press Left."); break;
       case IR_BUTTON_RIGHT: Serial.println("Press Right."); break;
       case IR_BUTTON_0: Serial.println("Press 0."); break;
       case IR_BUTTON_1: Serial.println("Press 1."); break;
       case IR_BUTTON_2: Serial.println("Press 2."); break;
       case IR_BUTTON_3: Serial.println("Press 3."); break;
       case IR_BUTTON_4: Serial.println("Press 4."); break;
       case IR_BUTTON_5: Serial.println("Press 5."); break;
       case IR_BUTTON_6: Serial.println("Press 6."); break;
       case IR_BUTTON_7: Serial.println("Press 7."); break;
       case IR_BUTTON_8: Serial.println("Press 8."); break;
       case IR_BUTTON_9: Serial.println("Press 9."); break;
       default: break;
    }
  }
}
